// Copyright 2020 Real-Time Innovations, Inc. (RTI)
//
// Copyleft 2024, Black Sesame Technologies
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "dal_connextdds/dal_impl.hpp"

#include <algorithm>
#include <string>
#include <vector>
#include <stdexcept>

// #include "dal_dds_common/time_utils.hpp"



#define ROS_SERVICE_REQUESTER_PREFIX_STR "rq"
#define ROS_SERVICE_RESPONSE_PREFIX_STR  "rr"

const char * const ROS_TOPIC_PREFIX = "rt";
const char * const ROS_SERVICE_REQUESTER_PREFIX = ROS_SERVICE_REQUESTER_PREFIX_STR;
const char * const ROS_SERVICE_RESPONSE_PREFIX = ROS_SERVICE_RESPONSE_PREFIX_STR;
const char * const ROS_CFT_TOPIC_NAME_INFIX = "_ContentFilterTopic";

// std::string
// dal_connextdds_create_topic_name(
//   const char * prefix,
//   const char * topic_name,
//   const char * suffix,
//   bool avoid_ros_namespace_conventions)
// {
//   if (avoid_ros_namespace_conventions) {
//     return std::string(topic_name) + std::string(suffix);
//   } else {
//     return std::string(prefix) +
//            std::string(topic_name) +
//            std::string(suffix);
//   }
// }

// std::string
// dal_connextdds_create_topic_name(
//   const char * prefix,
//   const char * topic_name,
//   const char * suffix,
//   const dal_qos_profile_t * qos_policies)
// {
//   return dal_connextdds_create_topic_name(
//     prefix,
//     topic_name,
//     suffix,
//     qos_policies->avoid_ros_namespace_conventions);
// }

dcutils_ret_t
dcutils_uint8_array_copy(
  dcutils_uint8_array_t * const dst,
  const dcutils_uint8_array_t * const src)
{
  if (src->buffer_length > 0) {
    if (src->buffer_length > dst->buffer_capacity) {
      dcutils_ret_t rc =
        dcutils_uint8_array_resize(dst, src->buffer_length);

      if (DCUTILS_RET_OK != rc) {
        return rc;
      }
    }

    dst->buffer_length = src->buffer_length;
    memcpy(dst->buffer, src->buffer, src->buffer_length);
  } else {
    dst->buffer_length = 0;
  }

  return DCUTILS_RET_OK;
}

static inline void str_trim(
  const char * const s,
  const size_t s_len,
  size_t * const s_start_out,
  size_t * const s_end_out)
{
  size_t s_i = 0;

  for (; s_i < s_len && std::isspace(s[s_i]); s_i++) {
  }
  *s_start_out = s_i;

  for (; s_i < s_len && !std::isspace(s[s_i]); s_i++) {
  }
  *s_end_out = s_i;
}

/******************************************************************************
 * Type helpers
 ******************************************************************************/

static
std::string
dal_connextdds_create_type_name(
  const char * const message_namespace,
  const char * const message_name,
  const char * const message_suffix,
  const bool mangle_prefix)
{
  const char * prefix_sfx = "";
  if (mangle_prefix) {
    prefix_sfx = "_";
  }

  std::ostringstream ss;
  std::string msg_namespace(message_namespace);
  if (!msg_namespace.empty()) {
    ss << msg_namespace << "::";
  }
  ss << "dds" << prefix_sfx << "::" << message_name << message_suffix;
  return ss.str();
}

std::string
dal_connextdds_create_type_name(
  const message_type_support_callbacks_t * callbacks,
  const bool mangle_names)
{
  const char * msg_prefix = "";
  const bool mangle_prefix = mangle_names;
  if (mangle_names) {
    msg_prefix = "_";
  }
  return dal_connextdds_create_type_name(
    callbacks->message_namespace_,
    callbacks->message_name_,
    msg_prefix,
    mangle_prefix);
}

std::string
dal_connextdds_create_type_name(
  const dalidl_typesupport_introspection_cpp::MessageMembers * const members,
  const bool mangle_names)
{
  const char * msg_prefix = "";
  const bool mangle_prefix = mangle_names;
  if (mangle_names) {
    msg_prefix = "_";
  }
  return dal_connextdds_create_type_name(
    members->message_namespace_,
    members->message_name_,
    msg_prefix,
    mangle_prefix);
}

std::string
dal_connextdds_create_type_name(
  const dalidl_typesupport_introspection_c__MessageMembers * const members,
  const bool mangle_names)
{
  const char * msg_prefix = "";
  const bool mangle_prefix = mangle_names;
  if (mangle_names) {
    msg_prefix = "_";
  }
  return dal_connextdds_create_type_name(
    members->message_namespace_,
    members->message_name_,
    msg_prefix,
    mangle_prefix);
}


